import processing.serial.*;
import processing.net.*;
import controlP5.*;

////////////////////////////////////////////////////////////////////
// Screen
////////////////////////////////////////////////////////////////////

ControlP5 controlP5;
Textarea myTextarea;
Textbuffer textbuffer;
public final boolean BOUT = true; 

////////////////////////////////////////////////////////////////////
// TCP: Scratch
////////////////////////////////////////////////////////////////////
Client scratchClient;


////////////////////////////////////////////////////////////////////
// SERIAL
////////////////////////////////////////////////////////////////////
Serial myPort; // The serial port
//int SERIAL_SPEED = 115200;
int SERIAL_SPEED = 38400;
int PORT_NR = 1;


////////////////////////////////////////////////////////////////////
// Rob
////////////////////////////////////////////////////////////////////
Robot myRob;


////////////////////////////////////////////////////////////////////
// CONSTANTS: Protocol definition: my actors and sensor IDs
////////////////////////////////////////////////////////////////////

public final byte TYPE_ACTOR = 0;
public final byte TYPE_SENSOR = 1;
public final byte TYPE_COMMANDER = 2;

public final byte ID_XDIVIDER = 0x01;
public final byte ID_XZEROZONE = 0x02;
public final byte ID_YZEROZONE = 0x03;
public final byte ID_PITCHZEROZONE = 0x04;
public final byte ID_ROLLZEROZONE  = 0x05;
public final byte ID_PITCHTOLERANCE = 0x06;
public final byte ID_ROLLTOLERANCE = 0x07;
public final byte ID_CHUCKACTORENABLER = 0x08;
public final byte ID_CHUCKACTORDELAY = 0x09;
public final byte ID_CHUCKSENSORENABLER = 0x0a;
public final byte ID_CHUCKSENSORDELAY = 0x0b;
public final byte ID_JOYX = 0x0c;
public final byte ID_JOYY = 0x0d;
public final byte ID_PITCH = 0x0e;
public final byte ID_ROLL = 0x0f;
public final byte ID_BTNC = 0x10;
public final byte ID_BTNZ = 0x11;
public final byte ID_INITIALIZER = 0x12;
public final byte ID_STATUSREQUESTER = 0x13;
public final byte ID_TELEMETRYENABLER = 0x14;
public final byte ID_TELEMETRYDELAY = 0x15;
public final byte ID_BATTERYSTATUS = 0x16;
public final byte ID_SERVOPANTARGET = 0x17;
public final byte ID_SERVOTILTTARGET = 0x18;
public final byte ID_SERVODELAY = 0x19;
public final byte ID_SERVOPANPOSITION = 0x1a;
public final byte ID_SERVOTILTPOSITION = 0x1b;
public final byte ID_ENGINESENSEDELAY = 0x1c;
public final byte ID_ENGINELPOWER = 0x1d;
public final byte ID_ENGINELDIRECTION = 0x1e;
public final byte ID_ENGINERPOWER = 0x1f;
public final byte ID_ENGINERDIRECTION = 0x20;
public final byte ID_ENGINELSPEED = 0x21;
public final byte ID_ENGINERSPEED = 0x22;
public final byte ID_IRBARENABLER = 0x23;
public final byte ID_IR1DISTANCE = 0x24;
public final byte ID_IR2DISTANCE = 0x25;
public final byte ID_IR3DISTANCE = 0x26;
public final byte ID_IR4DISTANCE = 0x27;
public final byte ID_IR5DISTANCE = 0x28;
public final byte ID_PINGDISTANCE = 0x29;
public final byte ID_PINGENABLER = 0x2a;
public final byte ID_SOUNDPLAY = 0x2b;
public final byte ID_CAM_ENABLER = 0x2c;
public final byte ID_MOCS_TURN = 0x2d;
//public final byte ID_MOCS_TURNL = 0x2e;
public final byte ID_MOCS_DRIVESEC = 0x2f;
//public final byte ID_MOCS_REVSEC = 0x30;
public final byte ID_CAVS_ENABLER = 0x31;
public final byte ID_FAVS_ENABLER = 0x32;
public final byte ID_HEARTBEAT = 0x33;
public final byte ID_MOVEPANTILT = 0x34;
public final byte ID_BTNCZ = 0x35;
public final byte ID_TELEMETRY =  0x36;  // the telemetry sensor data from rob to rcu
public final byte ID_LIGHTS =   0x37;  // rob actor: lightsystem
public final byte ID_BTNS =   0x38;  // rob sensor: buttonstate


////////////////////////////////////////////////////////////////////
// Protocol-Definition: my actors and sensors
////////////////////////////////////////////////////////////////////

void initRobController() {
  myRob.addController("nunchuck", TYPE_ACTOR, ID_XDIVIDER, "aChuckXDivider", 1, 128, 100 );
  myRob.addController("nunchuck", TYPE_ACTOR, ID_XZEROZONE, "aChuckXZerozone", 0, 90, 10 );
  myRob.addController("nunchuck", TYPE_ACTOR, ID_YZEROZONE, "aChuckYZerozone", 0, 90, 10 );
  myRob.addController("nunchuck", TYPE_ACTOR, ID_PITCHZEROZONE, "aChuckPitchZerozone", 0, 90, 20 );
  myRob.addController("nunchuck", TYPE_ACTOR, ID_ROLLZEROZONE, "aChuckRollZerozone", 0, 90, 20 );
  myRob.addController("nunchuck", TYPE_ACTOR, ID_PITCHTOLERANCE, "aChuckPitchTolerance", 0, 30, 10 );
  myRob.addController("nunchuck", TYPE_ACTOR, ID_ROLLTOLERANCE, "aChuckRollTolerance", 0, 30, 10 );
  myRob.addController("nunchuck", TYPE_ACTOR, ID_CHUCKACTORENABLER, "aChuckActorEnabler", 0, 1, 1 ); // (upWiichuck)
  myRob.addController("nunchuck", TYPE_ACTOR, ID_CHUCKACTORDELAY, "aChuckActorDelay", 0, 255, 20 ); // (upWiichuckDelay)
  myRob.addController("nunchuck", TYPE_ACTOR, ID_CHUCKSENSORENABLER, "aChuckSensorEnabler", 0, 1, 1 );// (dnWiichuck)
  myRob.addController("nunchuck", TYPE_ACTOR, ID_CHUCKSENSORDELAY, "aChuckSensorDelay", 0, 255, 10 ); // (dnWiichuckDelay)
  myRob.addController("nunchuck", TYPE_SENSOR, ID_JOYX, "sChuckJoyX", -127, 127 );
  myRob.addController("nunchuck", TYPE_SENSOR, ID_JOYY, "sChuckJoyY", -127, 127 );
  myRob.addController("nunchuck", TYPE_SENSOR, ID_PITCH, "sChuckPitch", -90, 90 );
  myRob.addController("nunchuck", TYPE_SENSOR, ID_ROLL, "sChuckRoll", -127, 127 );
  myRob.addController("nunchuck", TYPE_SENSOR, ID_BTNC, "sChuckBtnC", 0, 1 );
  myRob.addController("nunchuck", TYPE_SENSOR, ID_BTNZ, "sChuckBtnZ", 0, 1 );
  myRob.addController("robstatus", TYPE_COMMANDER, ID_INITIALIZER, "init", 0, 0, 0 ); //(sends init-command)
  myRob.addController("robstatus", TYPE_COMMANDER, ID_STATUSREQUESTER, "status", 0, 64, 0 ); //(sends ask-status-command) 0 = ask all actors and sensors
  myRob.addController("robstatus", TYPE_ACTOR, ID_TELEMETRYENABLER, "aRobTelemetryEnabler", 0, 1, 1 );  //(dnTelemetry)
  myRob.addController("robstatus", TYPE_ACTOR, ID_TELEMETRYDELAY, "aRobTelemetryDelay", 0, 255, 30 ); //(upDelay = Heartbeat for dnTelemetry)
  myRob.addController("robstatus", TYPE_SENSOR, ID_BATTERYSTATUS, "sRobBatteryStatus", 0, 255 );
  myRob.addController("pantilt", TYPE_ACTOR, ID_SERVOPANTARGET, "aRobServoPanTarget", 0, 180, 90 );
  myRob.addController("pantilt", TYPE_ACTOR, ID_SERVOTILTTARGET, "aRobServoTiltTarget", 30, 150, 90 );
  myRob.addController("pantilt", TYPE_ACTOR, ID_SERVODELAY, "aRobServoDelay", 0, 255, 5 );
  myRob.addController("pantilt", TYPE_SENSOR, ID_SERVOPANPOSITION, "sRobServoPanPosition", 0, 180 );
  myRob.addController("pantilt", TYPE_SENSOR, ID_SERVOTILTPOSITION, "sRobServoTiltPosition", 0, 180 );
  myRob.addController("engine", TYPE_ACTOR, ID_ENGINESENSEDELAY, "aRobEngineSenseDelay", 0, 255, 100 );
  myRob.addController("engine", TYPE_ACTOR, ID_ENGINELPOWER, "aRobEngineLPower", 0, 255, 0 );
  myRob.addController("engine", TYPE_ACTOR, ID_ENGINELDIRECTION, "aRobEngineLDirection", 0, 3, 0 );        // 0=STOP, 1 = FWD, 2=REV,  3=BREAK
  myRob.addController("engine", TYPE_ACTOR, ID_ENGINERPOWER, "aRobEngineRPower", 0, 255, 0 );
  myRob.addController("engine", TYPE_ACTOR, ID_ENGINERDIRECTION, "aRobEngineRDirection", 0, 3, 0 );        // 0=STOP, 1 = FWD, 2=REV,  3=BREAK
  myRob.addController("engine", TYPE_SENSOR, ID_ENGINELSPEED, "sRobEngineLSpeed", 0, 255 );
  myRob.addController("engine", TYPE_SENSOR, ID_ENGINERSPEED, "sRobEngineRSpeed", 0, 255 );
  myRob.addController("distance", TYPE_ACTOR, ID_IRBARENABLER, "aRobIrBarEnabler", 0, 1, 1 );
  myRob.addController("distance", TYPE_SENSOR, ID_IR1DISTANCE, "sRobIr1Distance", 0, 255 );
  myRob.addController("distance", TYPE_SENSOR, ID_IR2DISTANCE, "sRobIr2Distance", 0, 255 );
  myRob.addController("distance", TYPE_SENSOR, ID_IR3DISTANCE, "sRobIr3Distance", 0, 255 );
  myRob.addController("distance", TYPE_SENSOR, ID_IR4DISTANCE, "sRobIr4Distance", 0, 255 );
  myRob.addController("distance", TYPE_SENSOR, ID_IR5DISTANCE, "sRobIr5Distance", 0, 255 );
  myRob.addController("distance", TYPE_SENSOR, ID_PINGDISTANCE, "sRobPingDistance", 0, 255 );
  myRob.addController("distance", TYPE_ACTOR, ID_PINGENABLER, "aRobPingEnabler", 0, 1, 1 );
  myRob.addController("sound", TYPE_COMMANDER, ID_SOUNDPLAY, "soundPlay", 0, 7, 0 ); // soundnr
  myRob.addController("camera", TYPE_ACTOR, ID_CAM_ENABLER, "aRobCamEnabler", 0, 1, 0 );
  myRob.addController("mocs", TYPE_COMMANDER, ID_MOCS_TURN, "robTurn", 0, 180, 0 ); // motor controlling system
//  myRob.addController("mocs", TYPE_COMMANDER, ID_MOCS_TURNL, "robTurnL", 0, 180, 0 );
  myRob.addController("mocs", TYPE_COMMANDER, ID_MOCS_DRIVESEC, "robDriveSec", 0, 20, 0 );
//  myRob.addController("mocs", TYPE_COMMANDER, ID_MOCS_REVSEC, "robRevSec", 0, 20, 0 );
  myRob.addController("cavs", TYPE_ACTOR, ID_CAVS_ENABLER, "aRobCavsEnable", 0, 1, 0 ); // collision avoidance system 0=off > 0 =  prognr
  myRob.addController("favs", TYPE_ACTOR, ID_FAVS_ENABLER, "aRobFavsEnable", 0, 1, 0 ); // falling avoidance system 0=off > 0 =  prognr
  myRob.addController("robstatus", TYPE_COMMANDER, ID_HEARTBEAT, "heartbeat", 0, 0, 0 ); // heartbeat 4 rob
  myRob.addController("pantilt", TYPE_ACTOR, ID_MOVEPANTILT, "aMovePanTilt", 0, 64, 0 );  
  myRob.addController("nunchuck", TYPE_ACTOR, ID_BTNCZ, "aBtnCZ", 0, 2, 0 ); // btnstate
  myRob.addController("robstatus", TYPE_ACTOR, ID_LIGHTS, "aLights", 0, 16, 0 ); // rob lights
  myRob.addController("robstatus", TYPE_SENSOR, ID_BTNS, "sButtons", 0, 16, 0 ); // rob buttons
}

//TYPE_COMMANDs are broadcasts from scratch to this driver and flow always on call
//TYPE_ACTORSs flow from scratch, this and rcu to rob or to rcu only when value changes
//TYPE_SENSORSs flow from rob to scratch only when values change






////////////////////////////////////////////////////////////////////
// Setup and Loop
////////////////////////////////////////////////////////////////////

public void setup() {
  myRob = new Robot();
  initRobController();


  controlP5 = new ControlP5(this);
  size(300, 200);
  frameRate(100);
  //background(0);

  myTextarea = controlP5.addTextarea(
  "label1", 
  "a"
    , 10, 5, 280, 190);
  myTextarea.setColorForeground(0xffff0000);

  textbuffer = new Textbuffer(80);
  if(BOUT) textbuffer.enqueue("connecting...");
  setupSerialCommunication();
  setupNetCommunication();
  
  println("alive");
 
}


void draw() {
  background(0);
  myTextarea.setText(textbuffer.bulk());
}





////////////////////////////////////////////////////////////////////
// SETTING UP SCRATCH COMMUNICATION
////////////////////////////////////////////////////////////////////

void setupNetCommunication() {
  scratchClient = new Client (this, "127.0.0.1", 42001);
}




////////////////////////////////////////////////////////////////////
// RECEIVING FROM SCRATCH
////////////////////////////////////////////////////////////////////

// Event: received something from scratch
void clientEvent(Client someClient) {
  String dataIn;
  if (someClient.available() > 0) { // If there's incoming data from  the client...
    dataIn = someClient.readString(); // ...then grab it and print it
    String cleanData = dataIn.substring(4, dataIn.length() - 0);
    if(BOUT) textbuffer.enqueue("scr -> rcu: " + cleanData);
    dispatchScratchCmd(cleanData);
  }
}



void dispatchScratchCmd(String theCommand) {


  List<String> matchList = new ArrayList<String>();
  Pattern regex = Pattern.compile("[^\\s\"']+|\"([^\"]*)\"|'([^']*)'");
  Matcher regexMatcher = regex.matcher(theCommand);
  while (regexMatcher.find ()) {    
    if (regexMatcher.group(1) != null) {
      // Add double-quoted string without the quotes
      matchList.add(regexMatcher.group(1));
    } 
    else if (regexMatcher.group(2) != null) {
      // Add single-quoted string without the quotes
      matchList.add(regexMatcher.group(2));
    }
    else {
      // Add unquoted word
      matchList.add(regexMatcher.group());
    }
  } 

  for (int i=0; i < matchList.size(); i++) {
    String [] _match = match(matchList.get(i), "broadcast");
    if (_match != null) {  // found a broadcast command
      handleScratchBroadcast(matchList.get(++i));
    }
    else {
      _match = match(matchList.get(i).toString(), "sensor-update");
      if (_match != null)  // found a sensor-update command
            handleScratchSensorUpdate(matchList.get(++i), matchList.get(++i));
      else // another key value pair after a sensor-update
            handleScratchSensorUpdate(matchList.get(i), matchList.get(++i));
    }
  }
}



void handleScratchSensorUpdate(String theKey, String theVal) {
  
  //println ("au:" + theKey + ":" + theVal + "!");

  int _val = 0;
  if(theVal.length()>0)
    _val = int(theVal);
 
  RobotController rc = myRob.getControllerByName(theKey);
  if (rc != null && rc.type() == TYPE_ACTOR) { // found an actor
    if (rc.setActor(_val) != -1)
      if(BOUT) textbuffer.enqueue("handle Scratch: actor-update "+ theKey + " = " + rc.value() + " done");
    //else
      //textbuffer.enqueue("handle Scratch: actor-update "+ theKey + " unchanged");
  }
  //else {
    //if(BOUT) textbuffer.enqueue("handle Scratch: actor-update "+ theKey + " not in my list");
  //}
}


void handleScratchBroadcast(String theCommand) {
  
  //println ("bc:" + theCommand + "!");
  
  String[] _split = split(theCommand," ");
  String _var = _split[0];
  
  if (_split.length > 2){
    if (BOUT) textbuffer.enqueue("handle Scratch: broadcast "+ theCommand + " has to many arguments");
    return;
  }

  int _val = 0;
  if (_split.length == 2){
    if (_split[1].length() > 0)
      _val = int(_split[1]);
  }
  
  //println("bc: " + _var + ":" + _val + "!");
  
  RobotController rc = myRob.getControllerByName(_var);
  if (rc == null) { // not a valid actor
    if (BOUT) textbuffer.enqueue("handle Scratch: broadcast " + _var + " unkonown to me");
    return;
  }
  
  if (rc.id() == ID_INITIALIZER) {
    RobotController[] rca = myRob.getControllerList();
    for (int i = 0 ; i< rca.length; i++) {
      //if (rca[i].type() == TYPE_ACTOR) {
        rca[i].init();
        //textbuffer.enqueue("scratch actor-update "+ rca[i].name() + " = " + rca[i].value() + " done");
        //scratchSend("sensor-update \""+ rca[i].name() + "\" " + rca[i].value());
        //scratchSend("broadcast \"sensor-update "+ rca[i].name() + "\"");
        //textbuffer.enqueue("broadcast to scratch \"sensor-update "+ rca[i].name() + "\"");
      //}
    }
    return;
  }
  
  if ( rc.doCommander(_val) != -1) {
    if (BOUT) textbuffer.enqueue("handle Scratch: broadcast "+ _var + " = " + rc.value() + " done");
  }
  else {
    if (BOUT) textbuffer.enqueue("handle Scratch: broadcast " + _var + " not commander-type");
  }
}


////////////////////////////////////////////////////////////////////
// SENDING TO SCRATCH
////////////////////////////////////////////////////////////////////

// send to scratch
void scratchSend(String dataOut) {
  byte [] sizeBytes = { 
    0, 0, 0, 0
  };
  sizeBytes[3] = byte(dataOut.length());
  for (int i=0; i<4; i++) {
    scratchClient.write(sizeBytes[i]);
  }
  textbuffer.enqueue("rcu -> scr: " + dataOut);
  scratchClient.write(dataOut);
}


////////////////////////////////////////////////////////////////////
// SETTING UP RCU COMMUNICATION
////////////////////////////////////////////////////////////////////

// Setup Serial communication to rcu
void setupSerialCommunication() {
  println(Serial.list());
  myPort = new Serial(this, Serial.list()[PORT_NR], SERIAL_SPEED);
  myPort.buffer(2); // READ 16 bit allways
}



////////////////////////////////////////////////////////////////////
// RECEIVING FROM RCU
////////////////////////////////////////////////////////////////////


//int[] rcuRcvBuf = new int[35];
//byte rcuRcvBufPos = 0;
//byte rcuRcvBufLen = 0;
//int rcvCmd;

// Event: Received something from rcu
// Serial receive Event
void serialEvent(Serial p) {

  if(myPort.available() > 1) {
     int data = myPort.read();
     data = (data <<8) | myPort.read();
     dispatchRcuCmd (data);
  }
}

void dispatchRcuCmd(int theCommand) {

  int _cmd = decodeSer(theCommand);
  int _key = _cmd >> 8;
  int _val = _cmd & 0xFF;
  
//println("received from rcu: " + hex(theCommand,4) + "!");

  RobotController rc = myRob.getControllerByID(_key);
  if (rc == null) {
    if(BOUT) textbuffer.enqueue("dispatch RcuCmd: unknown ID " + hex(_key, 2));
    return;
  }
  //if (rc.type() == TYPE_ACTOR) {
    //rc.gotActor(_val);
//println("got actor");    
  //}
  //else { // TYPE_SENSOR
    rc.gotSensor(_val);
//println("got sensor");    
  //}

}




////////////////////////////////////////////////////////////////////
// SENDING TO RCU
////////////////////////////////////////////////////////////////////

// send something to rcu
void rcuSend(int theCommand) {
  if (myPort!=null) {
    myPort.write(theCommand >> 8);
    myPort.write(theCommand & 0xFF);
//    println("sending to rcu: " + hex(theCommand,4) + "!");
  }
  else
    println("sending to rcu: Serial not available");
   
}




////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////
// UTILITIES
////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////
// SERIAL ENCODER / DECODER UTILITIES
////////////////////////////////////////////////////////////////////



// Encode: Build Command-byte, data-byte with
// commandbyte 7 = 1
// commandbyte 6..1 = code
// commandbyte 9 = databyte bit 7
// databyte 7 = 0
// databyte 6..0 = data without high bit
public int encodeSer( int theCommand, int theData) {
  return(theCommand<<8 | theData);
  /*
  int _msg = 0;
  _msg = (((((_msg | (theCommand & 0x40)) << 8) | theData ) << 1 ) &
    0xFF00 | (theData & 0x7F));
  return _msg;
  */
}

// Decode:
// commandbyte | databyte
// commandbyte 5..0 = code
// databyte 7..0 = data
public int decodeSer (int theMsg) {
  return theMsg;
  /*
  int _msg;
  int _data;
  int _code;
  _data = ((theMsg >> 1) & 0x80) | (theMsg & 0x7F);
  _code = (theMsg>>9) & 0x3F;
  _msg = (((int)_code) <<8) | _data;
  return _msg;
  */
}




////////////////////////////////////////////////////////////////////
// Textbuffer utility class for Net Communication
////////////////////////////////////////////////////////////////////

class Textbuffer {
  private String[] a;          // queue elements
  private int N = 0;           // number of elements on queue
  private int first = 0;       // index of first element of queue
  private int last  = 0;       // index of next available slot

  public Textbuffer(int capacity) {
    a = new String[capacity];
  }

  public boolean isEmpty() { 
    return N == 0;
  }
  public int size() { 
    return N;
  }

  public void enqueue(String item) {
    a[last] = item;
    last = (last + 1) % a.length;     // wrap-around
    if (last == first)
      first = (first + 1) % a.length;     // wrap-around
    else
      N++;
  }

  public String dequeue() {
    if (isEmpty()) { 
      throw new RuntimeException("Ring buffer underflow");
    }
    String item = a[first];
    a[first] = null;                  // to help with garbage collection
    N--;
    first = (first + 1) % a.length;   // wrap-around
    return item;
  }

  public String bulk() {
    String result = new String();
    for (int i= first ; i!=last; i = (i+1) % a.length) {
      result = result + a[i] + "\n";
    }
    return result;
  }
}





////////////////////////////////////////////////////////////////////
// Robot Class
////////////////////////////////////////////////////////////////////

public class Robot {
  private int _id;
  private String _name;
  private HashMap<String, RobotController> _myControllerMap;
  private HashMap<Integer, String> _myControllerIDMap;

  Robot() {
    _myControllerMap = new HashMap<String, RobotController>();
    _myControllerIDMap = new HashMap<Integer, String>();
  }

  public RobotController addController( String theGroup, int theType, 
  byte theId, String theName, int theMin, int theMax) {
    return addController(theGroup, theType, theId, theName, theMin, theMax, 0);
  }

  public RobotController addController(
  String theGroup, int theType, byte theId, String theName, 
  int theMin, int theMax, int theDefault
    ) {
    RobotController myController = new RobotController(theGroup, 
    theType, theId, theName, theMin, theMax, theDefault);
    _myControllerMap.put(myController.name(), myController);
    _myControllerIDMap.put(new Integer(theId), myController.name());
    myController.init();
    return myController;
  }


  public RobotController getControllerByName(String theName) {
    if (_myControllerMap.containsKey(theName)) {
      if (_myControllerMap.get(theName) instanceof RobotController) {
        return (RobotController) _myControllerMap.get(theName);
      }
    }
    return null;
  }

  public RobotController getControllerByID(int theID) {
    if (_myControllerIDMap.containsKey(theID)) {
      if (_myControllerIDMap.get(theID) instanceof String) {
        return getControllerByName(_myControllerIDMap.get(theID));
      }
    }
    return null;
  }


  public RobotController[] getControllerList() {
    RobotController[] myControllerList = new
      RobotController[_myControllerMap.size()];
    _myControllerMap.values().toArray(myControllerList);
    return myControllerList;
  }

  public String name() {
    return _name;
  }

  public int id() {
    return _id;
  }

  public int numControllers() {
    return _myControllerMap.size();
  }
}


////////////////////////////////////////////////////////////////////
// Robot Controller Class
////////////////////////////////////////////////////////////////////

public class RobotController {
  private String _group;
  private int _type;
  private int _id;
  private String _name;
  private int _min;
  private int _max;
  private int _default;
  private int _value;

  RobotController(String theGroup, int theType, int theId, String
    theName, int theMin, int theMax, int theDefault) {
    _group = new String(theGroup);
    _type = theType;
    _id = theId;
    _name = new String(theName);
    _min = theMin;
    _max = theMax;
    _default = theDefault;
    _value = _default;
  }

  public void init() {
    _value = _default;
    if ( _type == TYPE_ACTOR )
      rcuSend(encodeSer(_id, _default));
  }

  public int setActor(int theValue) { // done by Scratch
    if ( _type != TYPE_ACTOR )
      return -1;
    if ( theValue == _value)
      return theValue;
    if ( theValue < _min )
      _value = _min;
    else if ( theValue > _max )
      _value = _max;
    else
      _value = theValue;
    rcuSend(encodeSer(_id, theValue));
    return theValue;
  }


  public int doCommander(int theValue) { // done by scratch
    if ( _type != TYPE_COMMANDER )
      return -1;
    if ( theValue < _min )
      _value = _min;
    else if ( theValue > _max )
      _value = _max;
    else
      _value = theValue;
    rcuSend(encodeSer(_id, theValue));
    return theValue;
  }


  public int gotSensor(int theValue) { // done by RCU
    //if ( _type != TYPE_SENSOR )
    //  return -1;

    if ( theValue == _value || (_min < 0 && (_value == signedByte(theValue)) ))
      return theValue;
      
    if(_min < 0)
      _value = signedByte(theValue);  
    else
      _value = theValue;  

    if ( _value < _min )
      _value = _min;
    else if ( _value > _max )
      _value = _max;
  
    scratchSend("sensor-update \""+ _name + "\" " + _value);
    if (BOUT) textbuffer.enqueue("handle RCU sensor-update \""+ _name + "\" " + _value);
    scratchSend("broadcast \"sensor-update "+ _name + "\"");
    if (BOUT) textbuffer.enqueue("handle RCU broadcast \"sensor-update "+ _name + "\"");
    return theValue;
  }

/*
  public int gotActor(int theValue) { // done by RCU
    if ( _type != TYPE_ACTOR )
      return -1;
    if ( theValue == _value)
      return theValue;
    if ( theValue < _min )
      _value = _min;
    else if ( theValue > _max )
      _value = _max;
    else
      _value = theValue;
    scratchSend("sensor-update \""+ _name + "\" " + _value);
    textbuffer.enqueue("sensor-update \""+ _name + "\" " + _value);
    scratchSend("broadcast \"sensor-update "+ _name +  "\"");
    textbuffer.enqueue("broadcast \"sensor-update "+ _name + "\"");
    return theValue;
  }
*/

  public String name () {
    return _name;
  }

  public int id() {
    return _id;
  }

  public int value() {
    return _value;
  }

  public int type() {
    return _type;
  }
  
  private byte signedByte( int val ) { 
    return (byte)( val > 127 ? val - 256 : val );
  }
}

